# encoding:utf-8
import math
import random
import EventLog as log
import time
import matplotlib.pyplot as plt

class ElbowJoint:
    #肘关节转换
    def __init__(self):
        self.zerol = 209.5
        self.minl = 209
        self.maxl = 263.5
        self.xlist = []
        self.ylist = []
        self.der = 51  # 核心角度+der = 机器人关节角度
        for l in range(20900, 26350, 10):
            l1 = l * 1.0 / 100
            r = self.calculate_ElbowJoint(l1)
            self.xlist.append(l1)
            self.ylist.append(r)

    def __calculate_angles(self, side_a, side_b, side_c):
        # 计算三角形的三个角度（单位为弧度）
        angle_A = math.acos((side_b ** 2 + side_c ** 2 - side_a ** 2) / (2 * side_b * side_c))
        angle_B = math.acos((side_a ** 2 + side_c ** 2 - side_b ** 2) / (2 * side_a * side_c))
        angle_C = math.pi - angle_A - angle_B
        # 将弧度转换为角度
        angle_A = math.degrees(angle_A)
        angle_B = math.degrees(angle_B)
        angle_C = math.degrees(angle_C)
        return angle_A, angle_B, angle_C

    def calculate_ElbowJoint(self, l):
        # 已知丝杆长度，求肘关节角度
        a1, b1, c1 = self.__calculate_angles(231.35, 38.18, l)
        return c1

    def __find_nearest_number(self, lst, target):
        nearest_number = min(lst, key=lambda x: abs(x - target))
        index = lst.index(nearest_number)
        return nearest_number, index

    def angle2lenght(self, in_angle):
        # 给出一个关节角度，得到电机长度
        nearest_number, index = self.__find_nearest_number(self.ylist, self.der+in_angle)
        return self.xlist[index]

    def lenght2angle(self, in_lenght):
        # 给出电机的长度，关节的角度
        nearest_number, index = self.__find_nearest_number(self.xlist, in_lenght)
        return self.ylist[index]-self.der
    def __del__(self):
        pass

class WristJoint:
    # 手腕部关节
    def __init__(self):
        self.zerol = 174.42  # 两个丝杆最大值最小值一样
        self.minl = 159 # 160(实际机械设置数值)
        self.maxl = 197 # 196(实际机械设置数值)
        self.xlist = []
        self.ylist = []
        self.der6 = 103  # 核心角度+der = 机器人关节角度
        for la in range(self.minl*10, self.maxl*10, 1):
            for lb in range(self.minl*10, self.maxl*10, 1):
                if abs(la+lb)>1900*2:continue   # 两个丝杆平均长度不能大于绕轴的两个边的总长度
                if abs(la+lb)<1460*2:continue   # 两个丝杆平均长度不能小于绕轴的两个边的总长度
                la1 = la * 1.0 / 10
                lb1 = lb * 1.0 / 10
                r5,r6 = self.calculate_WristJoint(la1,lb1)
                self.xlist.append((la1,lb1))
                self.ylist.append((r5,r6))

    def __calculate_angles(self,side_a, side_b, side_c):
        # 计算三角形的三个角度（单位为弧度）
        angle_A = math.acos((side_b ** 2 + side_c ** 2 - side_a ** 2) / (2 * side_b * side_c))
        angle_B = math.acos((side_a ** 2 + side_c ** 2 - side_b ** 2) / (2 * side_a * side_c))
        angle_C = math.pi - angle_A - angle_B
        # 将弧度转换为角度
        angle_A = math.degrees(angle_A)
        angle_B = math.degrees(angle_B)
        angle_C = math.degrees(angle_C)
        return angle_A, angle_B, angle_C

    def __find_nearest_point(self,points, px,py):
        p = [px,py]
        min_distance = math.inf
        nearest_index = None
        for i, point in enumerate(points):
            distance = math.sqrt((point[0] - p[0]) ** 2 + (point[1] - p[1]) ** 2)
            if distance < min_distance:
                min_distance = distance
                nearest_index = i
        return min_distance,nearest_index

    def calculate_WristJoint(self,la, lb):
        #计算关节角度
        lm = 0.5*(la + lb)
        ld = la-lb
        _,_,c = self.__calculate_angles(167.8426, 22.559, lm)
        degrees = math.degrees(0.5*ld/(62*0.5))
        angle6 = c
        angle7 = degrees
        return angle6,angle7

    def angle2lenght(self,in_angle6,in_angle7):
        # 给出一个关节角度，得到点击长度
        # in_angle7不能超过限度正负
        t1 = time.time()
        if in_angle7>12:
            log.logger.warning("左右摆动角度不能大于12，实际为%s"%in_angle7)
            in_angle7 = 12
        if in_angle7<-12:
            log.logger.warning("左右摆动角度不能大于-12，实际为%s"%in_angle7)
            in_angle7 = -12
        distance,index = self.__find_nearest_point(self.ylist, in_angle6+self.der6,in_angle7)
        # print("distance",distance)
        return self.xlist[index][0],self.xlist[index][1]

    def lenght2angle(self,in_lenght1,in_lenght2):
        # 给出电机的长度，关节的角度
        # 不能超过限度正负15
        if abs(in_lenght1-in_lenght2)>15:
            log.logger.warning("读角度：两个丝杆长度不能超过15，实际为%s" % (in_lenght1-in_lenght2))
        distance,index = self.__find_nearest_point(self.xlist, in_lenght1,in_lenght2)
        # print("distance",distance)
        return self.ylist[index][0]-self.der6,-self.ylist[index][1]

    def __del__(self):
        pass

def test_ElbowJoint():
    # 肘关节测试
    elbow = ElbowJoint()
    # 测试肘变化角度
    change_rod = 240
    angle4 = elbow.calculate_ElbowJoint(change_rod)
    print(angle4)
    minangle = elbow.calculate_ElbowJoint(209.5)
    maxangle = elbow.calculate_ElbowJoint(263.5)
    print("最小角度：", minangle, "丝杆最小长度：", 209.5)
    print("最大角度：", maxangle, "丝杆最大长度：", 263.5)

    # 2.2.通过长度计算关节变化角度（根据angleList和lenghtList取最邻近点）

    rlt_angle = elbow.lenght2angle(263.5)
    print(rlt_angle)
    # 2.3.通过长度计算角度（根据angleList和lenghtList取最邻近点）

    rlt_lenght20 = elbow.angle2lenght(20)
    print("rlt_lenght20=", rlt_lenght20)
    angle20 = elbow.lenght2angle(rlt_lenght20)
    print("angle20=", angle20)

    rlt_lenght0 = elbow.angle2lenght(0)
    print("rlt_lenght0=", rlt_lenght0)
    angle0 = elbow.lenght2angle(rlt_lenght0)
    print("angle0=", angle0)

    rlt_lenght49 = elbow.angle2lenght(49)
    print("rlt_lenght49=", rlt_lenght49)
    angle49 = elbow.lenght2angle(rlt_lenght49)
    print("angle49=", angle49)

    rlt_lenght93 = elbow.angle2lenght(93)
    print("rlt_lenght93=", rlt_lenght93)
    angle93 = elbow.lenght2angle(rlt_lenght93)
    print("angle93=", angle93)

    # lenghtList = elbow.xlist
    # angleList = elbow.ylist
    # y1list = []
    # y2list = []
    # for x, y in zip(lenghtList, angleList):
    #     y1list.append(x)
    #     y2list.append(y)
    # plt.scatter(y1list, y2list)
    # plt.xlabel('length')
    # plt.ylabel('angle4')
    # plt.show()

def test_WristJoint():
    #手腕关节测试
    wrist = WristJoint()
    angle6, angle7 = wrist.calculate_WristJoint(180, 180)
    print(angle6, angle7)
    minangle6, minangle7 = wrist.calculate_WristJoint(148, 148)
    print("最小最小角度：", minangle6-103, minangle7, "丝杆长度：", 148, 148)
    minangle6, minangle7 = wrist.calculate_WristJoint(190, 148)
    print("最大最小角度：", minangle6-103, minangle7, "丝杆长度：", 190, 148)
    minangle6, minangle7 = wrist.calculate_WristJoint(148, 190)
    print("最小最大角度：", minangle6-103, minangle7, "丝杆长度：", 148, 190)
    minangle6, minangle7 = wrist.calculate_WristJoint(190, 190)
    print("最大最大角度：", minangle6-103, minangle7, "丝杆长度：", 190, 190)

    minangle6, minangle7 = wrist.calculate_WristJoint(170 + 20, 170 - 20)
    print("踝关节摆动角度：", minangle6, minangle7, "丝杆长度：", 190, 150)

    length6_, length7_ = wrist.angle2lenght(0,0)
    print("7,0:",length6_, length7_)
    angle6_, angle7_ = wrist.lenght2angle(length6_, length7_)
    print("7,0:",angle6_, angle7_)

    length6_, length7_ = wrist.angle2lenght(0,10)
    print("7,10:",length6_, length7_)
    angle6_, angle7_ = wrist.lenght2angle(length6_, length7_)
    print("7,10:",angle6_, angle7_)

    length6_, length7_ = wrist.angle2lenght(0,18)
    print("7,18:",length6_, length7_)
    angle6_, angle7_ = wrist.lenght2angle(length6_, length7_)
    print("7,18:",angle6_, angle7_)

    length6_, length7_ = wrist.angle2lenght(0,0)
    print("6,0:",length6_, length7_)
    angle6_, angle7_ = wrist.lenght2angle(length6_, length7_)
    print("6,0:",angle6_, angle7_)

    length6_, length7_ = wrist.angle2lenght(20,0)
    print("6,20:",length6_, length7_)
    angle6_, angle7_ = wrist.lenght2angle(length6_, length7_)
    print("6,20:",angle6_, angle7_)

    length6_, length7_ = wrist.angle2lenght(50,0)
    print("6,50:",length6_, length7_)
    angle6_, angle7_ = wrist.lenght2angle(length6_, length7_)
    print("6,50:",angle6_, angle7_)

    length6_, length7_ = wrist.angle2lenght(64,0)
    print("6,64:",length6_, length7_)
    angle6_, angle7_ = wrist.lenght2angle(length6_, length7_)
    print("6,64:",angle6_, angle7_)

    lenghtList = wrist.xlist
    angleList = wrist.ylist
    y1list = []
    y2list = []
    for y in angleList:
        if abs(y[1])>15:continue
        y1list.append(y[0]-103)
        y2list.append(y[1])
    # plt.scatter(y1list, y2list)
    # plt.xlabel('angle6')
    # plt.ylabel('angle7')

    fig1 = plt.figure()
    ax1 = fig1.add_subplot(111)
    ax1.scatter(y1list, y2list)
    plt.xlabel('angle6:left-right')
    plt.ylabel('angle7:up-down')

    plt.show()

if __name__ == "__main__":
    #肘部关节角度范围[0,93]
    test_ElbowJoint()
    #腕部关节角度范围angle6[-76,66],angle7[-38,38]
    test_WristJoint()


